/*=========================================================================
UberSim Source Code Release
-------------------------------------------------------------------------
Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
                   James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
                   Erick Tryzelaar {erickt}@andrew.cmu.edu
School of Computer Science, Carnegie Mellon University
-------------------------------------------------------------------------
This software is distributed under the GNU General Public License,
version 2.  If you do not have a copy of this licence, visit
www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
in the hope that it will be useful, but WITHOUT ANY WARRANTY,
including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
-------------------------------------------------------------------------*/

#include <stdio.h>

#include "mmgr.h"

#include "MotionController.h"
#include "Node.h"
#include "Spatial.h"

//#include "RigidBody.h"

/*******************************/

Spatial::Spatial (char* name)
{
	this->name   = name;
	this->parent = 0;

	this->motionController = 0;

	isCalculatingTransform = true;

	localScale    = 1;
	localRotation = Matrix33 (Matrix33::IDENTITY);
	localPosition = Vector3  (0, 0, 0);

	worldScale    = 1;
	worldRotation = Matrix33 (Matrix33::IDENTITY);
	worldPosition = Vector3  (0, 0, 0);
}

/*******************************/

Spatial::~Spatial ()
{
	if (parent)
	{
		parent->removeChild (this);
	}

	if (motionController)
	{
		delete motionController;
	}
}

/*******************************/

char* Spatial::getName ()
{
	return name;
}

/*******************************/

Node* Spatial::getParent ()
{
	return parent;
}

/*******************************/

MotionController* Spatial::getMotionController ()
{
	return motionController;
}

/*******************************/

BoundingSphere& Spatial::getWorldBound ()
{
	return worldBound;
}

/*******************************/

Real& Spatial::getLocalScale ()
{
	return localScale;
}

/*******************************/

Matrix33& Spatial::getLocalRotation ()
{
	return localRotation;
}

/*******************************/

Vector3& Spatial::getLocalPosition ()
{
	return localPosition;
}

/*******************************/

Vector3 Spatial::getLocalLinearVelocity ()
{
	return localLinearVelocity;
}

/*******************************/

Vector3 Spatial::getLocalAngularVelocity ()
{
	return localAngularVelocity;
}

/*******************************/

Real& Spatial::getWorldScale ()
{
	return worldScale;
}

/*******************************/

Matrix33& Spatial::getWorldRotation ()
{
	return worldRotation;
}

/*******************************/

Vector3& Spatial::getWorldPosition ()
{
	return worldPosition;
}

/*******************************/

Vector3 Spatial::getWorldLinearVelocity ()
{
	return worldLinearVelocity;
}

/*******************************/

Vector3 Spatial::getWorldAngularVelocity ()
{
	return worldAngularVelocity;
}

/*******************************/

void Spatial::setParent (Node* parent)
{
	this->parent = parent;
}

/*******************************/

void Spatial::setMotionController (MotionController* motionController)
{
	this->motionController = motionController;
}

/*******************************/

void Spatial::setLocalScale (const Real localScale)
{
	this->localScale = localScale;
}

/*******************************/

void Spatial::setLocalRotation (const Matrix33& localRotation)
{
	this->localRotation = localRotation;
}

/*******************************/

void Spatial::setLocalPosition (const Vector3& localPosition)
{
	this->localPosition = localPosition;
}


/*******************************/

void Spatial::setLocalLinearVelocity (const Vector3& localLinearVelocity)
{
	
}

/*******************************/

void Spatial::setLocalAngularVelocity (const Vector3& localAngularVelocity)
{
}

/*******************************/

void Spatial::setWorldScale (const Real worldScale)
{
	this->worldScale = worldScale;
}

/*******************************/

void Spatial::setWorldRotation (const Matrix33& worldRotation)
{
	this->worldRotation = worldRotation;
}

/*******************************/

void Spatial::setWorldPosition (const Vector3& worldPosition)
{
	this->worldPosition = worldPosition;
}

/*******************************/

void Spatial::setWorldLinearVelocity (const Vector3& worldLinearVelocity)
{
	
}

/*******************************/

void Spatial::setWorldAngularVelocity (const Vector3& worldAngularVelocity)
{
}

/*******************************/

void Spatial::updateGeometricState (Real deltaTime)
{
	updateWorldData  (deltaTime);
	updateWorldBound ();

/*	
	printf ("%s's worldbound: xyz: %f :%f :%f r: %f\n", 
		name, 
		worldBound.getCenter ().x, 
		worldBound.getCenter ().y, 
		worldBound.getCenter ().z, 
		worldBound.getRadius ());
*/
}

/*******************************/

void Spatial::updateWorldData (Real deltaTime)
{
	if (motionController)
	{
		motionController->update (deltaTime);
	}
	
	if (isCalculatingTransform)
	{
		if (parent)
		{
			worldScale    = parent->worldScale    * localScale;
			worldRotation = parent->worldRotation * localRotation;
			worldPosition = parent->worldPosition +
				parent->worldScale * (parent->worldRotation * localPosition);
		}
		else
		{
			worldScale    = localScale;
			worldRotation = localRotation;
			worldPosition = localPosition;
		}
	}

	//printf ("local position: %s %f %f %f\n", name, localPosition.x, localPosition.y, localPosition.z);
	//printf ("world position: %s %f %f %f\n", name, worldPosition.x, worldPosition.y, worldPosition.z);
}

/*******************************/

void Spatial::onCollidingWith (Spatial* object)
{
	if (worldBound.isCollidingWith (object->getWorldBound ()))
	{
		collidingWith (object);
	}
}

/*******************************/
